# (0.23920420624315739, -0.15600731369107962, 1.455)

import numpy as np

# 定义变换矩阵 T_v_cam
T_v_cam = np.array([
    [0.00274878, -0.141637, 0.989915, 0.466449],
    [-0.999923, 0.0115661, 0.00443145, 0.0242619],
    [-0.0120771, -0.989851, -0.141595, 0.111014],
    [0.0, 0.0, 0.0, 1.0]
])

# T_cam_v = np.linalg.inv(T_v_cam)
T_cam_v = T_v_cam

p_cam = np.array([0.239043737411499, -0.14727362823486329, 1.408 , 1.0])  # 示例点 (x, y, z, 1)

p_v = np.dot(T_cam_v, p_cam) #  
# 输出结果
print("Point in robot coordinate system:", p_v)
